#include <Arduino.h>
#include <SharpIR.h>
#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/LaserScan.h>

#define IR_MODEL   (1080) //1080 for GP2Y0A21Y 10 cm to 80 cm

struct Ranger
{
  double Q, R, P;    // KF
  double last_range; /**< Internal buffer to store the latest readings    */
};

ros::NodeHandle  nh;
sensor_msgs::LaserScan scan;
ros::Publisher pub_ir_scan( "ir_scan", &scan);
Ranger *rangers;

const int analog_pins[] = {A0, A1, A2, A3, A4, A5, A6, A7, A8, };
const int rangers_count = 9;

const float range_variance = 0.038f;
const float range_percent = 0.7f;
const float maximum_range = 0.65f;
const float infinity_range = 6.0f;
const float ir_ring_radius = 0.27f;
const float read_frequency = 20.0f;
const char *ir_frame_id = "ir_link";
unsigned long long seq = 0;

SharpIR *sharp_irs[rangers_count];

void computeRangesAndIntensity(unsigned int i, int dis)
{
  double v = dis;
  double r = v / 100.0;
  // KF - estimate - prediction
  rangers[i].P = rangers[i].P + rangers[i].Q;

  // KF - correction
  double z = r - rangers[i].last_range;
  double Z = rangers[i].R + rangers[i].P;

  double K = rangers[i].P/Z;

  double distanceKF = rangers[i].last_range + K*z;
  rangers[i].P = rangers[i].P - K*rangers[i].P;

  // KF - collect data
  rangers[i].last_range = distanceKF;

  // Fill range/intensity vectors inverting readings, as laser scans add beams from right (angle_min)
  // to left (angle_max), while our sensors are arranged from left (A0 port) to right (A10 port)
  scan.ranges[rangers_count - (i + 1)] = distanceKF <= maximum_range ? distanceKF + ir_ring_radius : infinity_range;
  scan.intensities[rangers_count - (i + 1)] = v;
}

void setup() {
  nh.initNode();
  nh.advertise(pub_ir_scan);

  scan.ranges = (float *)malloc(sizeof(float) * rangers_count);
  scan.intensities = (float *)malloc(sizeof(float) * rangers_count);

  scan.angle_min = -M_PI/2.0*range_percent;
  scan.angle_max = +M_PI/2.0*range_percent;
  scan.angle_increment = (scan.angle_max - scan.angle_min) / (double)(rangers_count - 1);
  scan.scan_time = 1.0 / read_frequency;
  scan.range_min = 0.10 + ir_ring_radius;
  scan.range_max = 11.00 + ir_ring_radius;

  scan.ranges_length = rangers_count;
  scan.intensities_length = rangers_count;

  scan.header.frame_id = ir_frame_id;

  rangers = (Ranger *)malloc(sizeof(Ranger) * rangers_count);
  for (unsigned int i = 0; i < rangers_count; i++)
  {
    rangers[i].last_range = maximum_range;
    rangers[i].Q = 0.001;
    rangers[i].R = range_variance;
    rangers[i].P = rangers[i].R;
  }

  //init sharp_irs
  for (unsigned int i = 0; i < rangers_count; i++)
  {
    int ir_pin = analog_pins[i];
    sharp_irs[i] = new SharpIR(ir_pin, IR_MODEL);
  }
}

void loop() {
  for (unsigned int i = 0; i < rangers_count; i++)
  {
    int dis = sharp_irs[i]->distance();
    computeRangesAndIntensity(i, dis);
  }
  scan.header.seq = seq ++;
  scan.header.stamp = nh.now();
  pub_ir_scan.publish(&scan);
  
  nh.spinOnce();
}
